﻿using DroneLib;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;

namespace DroneLib
{
    public class WaypointFile
    {
        public string Version { get; set; }
        public PointLatLngAlt Home { get; set; }
        public double HomeLatitude => Home.Lat;
        public double HomeLongitude => Home.Lng;
        public double HomeAltitude => Home.Alt;
        private List<MAVLink.mavlink_mission_item_int_t> _MissionItemInts = new List<MAVLink.mavlink_mission_item_int_t>();
        public List<MAVLink.mavlink_mission_item_int_t> MissionItemInts => _MissionItemInts;

        private List<MAVLink.mavlink_mission_item_t> _MissionItems = new List<MAVLink.mavlink_mission_item_t>();
        public List<MAVLink.mavlink_mission_item_t> MissionItems => _MissionItems;

        private WaypointFile() { }

        public WaypointFile(double homeLat, double homeLng, double homeAlt, params MAVLink.mavlink_mission_item_int_t[] items)
        {
            if (items.Length == 0)
                throw new Exception("航点数量不能为0");

            //WaypointFile file = new WaypointFile();
            this.Version = "110";

            if (homeLat != 0 && homeLng != 0)
            {
                this.Home = new PointLatLngAlt(homeLat, homeLng, homeAlt);
                //MAVLink.mavlink_mission_item_int_t homeItemInt = new MAVLink.mavlink_mission_item_int_t()
                //{
                //    command = (UInt16)MAVLink.MAV_CMD.WAYPOINT,
                //    param1 = 0,
                //    param2 = 0,
                //    param3 = 0,
                //    param4 = 0,
                //    x = (int)(homeLat * 1e7),
                //    y = (int)(homeLng * 1e7),
                //    z = (int)homeAlt,
                //    autocontinue = 1,
                //    current = 0,
                //    frame = (byte)MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT,
                //    mission_type = (byte)MAVLink.MAV_MISSION_TYPE.MISSION,
                //    seq = 0,
                //    target_component = items[0].target_component,
                //    target_system = items[0].target_system
                //};
                //_MissionItemInts.Add(homeItemInt);
            }
            _MissionItemInts.AddRange(items);


            foreach(var itemInt in _MissionItemInts)
            {
                MAVLink.mavlink_mission_item_t item = new MAVLink.mavlink_mission_item_t()
                {
                    autocontinue = itemInt.autocontinue,
                    command = itemInt.command,
                    current = itemInt.current,
                    frame = itemInt.frame,
                    mission_type = itemInt.mission_type,
                    param1 = itemInt.param1,
                    param2 = itemInt.param2,
                    param3 = itemInt.param3,
                    param4 = itemInt.param4,
                    x = (itemInt.command == (byte)MAVLink.MAV_CMD.WAYPOINT || itemInt.command == (byte)MAVLink.MAV_CMD.TAKEOFF || itemInt.command == (byte)MAVLink.MAV_CMD.VTOL_TAKEOFF) ? itemInt.x / 1e7f : itemInt.x,
                    y = (itemInt.command == (byte)MAVLink.MAV_CMD.WAYPOINT || itemInt.command == (byte)MAVLink.MAV_CMD.TAKEOFF || itemInt.command == (byte)MAVLink.MAV_CMD.VTOL_TAKEOFF) ? itemInt.y / 1e7f : itemInt.y,
                    z = itemInt.z,
                    seq = itemInt.seq,
                    target_component = itemInt.target_component,
                    target_system = itemInt.target_system
                };
                MissionItems.Add(item);
            }
        }

        public static WaypointFile Load(string filePath) 
        {
            if (File.Exists(filePath) == false)
                return null;

            var lines = File.ReadAllLines(filePath);
            return Load(lines);
        }

        public static WaypointFile Load(string[] lines)
        {
            if (lines[0].StartsWith("QGC WPL") == false)
                return null;

            WaypointFile file = new WaypointFile();
            file.Version = lines[0].Split(' ')[2];

            var items = lines[1].Split('\t');
            float lat = float.Parse(items[8]);
            float lng = float.Parse(items[9]);
            float alt = float.Parse(items[10]);
            file.Home = new PointLatLngAlt(lat, lng, alt);

            for (int i = 1; i < lines.Length; i++)
            {
                var line = lines[i];
                items = line.Split('\t');

                MAVLink.mavlink_mission_item_t missionItem = new MAVLink.mavlink_mission_item_t()
                {
                    seq = UInt16.Parse(items[0]),
                    current = byte.Parse(items[1]),
                    frame = byte.Parse(items[2]),
                    command = UInt16.Parse(items[3]),
                    param1 = float.Parse(items[4]),
                    param2 = float.Parse(items[5]),
                    param3 = float.Parse(items[6]),
                    param4 = float.Parse(items[7]),
                    x = float.Parse(items[8]),
                    y = float.Parse(items[9]),
                    z = float.Parse(items[10]),
                    autocontinue = byte.Parse(items[11]),
                };
                file.MissionItems.Add(missionItem);

                MAVLink.mavlink_mission_item_int_t missionItemInt = new MAVLink.mavlink_mission_item_int_t()
                {
                    seq = UInt16.Parse(items[0]),
                    current = byte.Parse(items[1]),
                    frame = byte.Parse(items[2]),
                    command = UInt16.Parse(items[3]),
                    param1 = float.Parse(items[4]),
                    param2 = float.Parse(items[5]),
                    param3 = float.Parse(items[6]),
                    param4 = float.Parse(items[7]),
                    x = 0,
                    y = 0,
                    z = float.Parse(items[10]),
                    autocontinue = byte.Parse(items[11]),
                };
                switch ((MAVLink.MAV_FRAME)missionItemInt.frame)
                {
                    case MAVLink.MAV_FRAME.LOCAL_ENU:
                    case MAVLink.MAV_FRAME.LOCAL_FRD:
                    case MAVLink.MAV_FRAME.LOCAL_FLU:
                    case MAVLink.MAV_FRAME.LOCAL_NED:
                    case MAVLink.MAV_FRAME.LOCAL_OFFSET_NED:
                        {
                            missionItemInt.x = (int)(float.Parse(items[8]) * 1e4);
                            missionItemInt.y = (int)(float.Parse(items[9]) * 1e4);
                            break;
                        }
                    case MAVLink.MAV_FRAME.GLOBAL:
                    case MAVLink.MAV_FRAME.GLOBAL_INT:
                    case MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT:
                    case MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT:
                    case MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT:
                    case MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT:
                        {
                            missionItemInt.x = (int)(float.Parse(items[8]) * 1e7);
                            missionItemInt.y = (int)(float.Parse(items[9]) * 1e7);
                            break;
                        }

                }
                file.MissionItemInts.Add(missionItemInt);

            }

            return file;
        }

        public void Save(string filePath)
        {
            StringBuilder sb = new StringBuilder();
            sb.AppendLine($"QGC WPL {Version}");
            sb.AppendLine($"0\t1\t6\t16\t0\t0\t0\t0\t{HomeLatitude}\t{HomeLongitude}\t{HomeAltitude}\t1");
            foreach(var item in _MissionItemInts)
            {
                float x=0, y=0;
                switch ((MAVLink.MAV_FRAME)item.frame)
                {
                    case MAVLink.MAV_FRAME.LOCAL_ENU:
                    case MAVLink.MAV_FRAME.LOCAL_FRD:
                    case MAVLink.MAV_FRAME.LOCAL_FLU:
                    case MAVLink.MAV_FRAME.LOCAL_NED:
                    case MAVLink.MAV_FRAME.LOCAL_OFFSET_NED:
                        {
                            x = item.x / 1e4f;
                            y = item.y / 1e4f;
                            break;
                        }
                    case MAVLink.MAV_FRAME.GLOBAL:
                    case MAVLink.MAV_FRAME.GLOBAL_INT:
                    case MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT:
                    case MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT:
                    case MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT:
                    case MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT_INT:
                        {
                            x = item.x / 1e7f;
                            y = item.y / 1e7f;
                            break;
                        }
                }
                sb.AppendLine($"{item.seq}\t0\t{item.frame}\t{item.command}\t{item.param1}\t{item.param2}\t" +
                    $"{item.param3}\t{item.param4}\t{x}\t{y}\t{item.z}\t{item.autocontinue}");
            }
            File.WriteAllText(filePath,sb.ToString());
        }
    }
}
